#ifndef PHYSICS_H
#define PHYSICS_H

#include "Maths.h"

class CWorld;

//Basically just a wrapper for a vector but also has an angle
//Entities are attached to these
class CPoint
{
public:
    
    //TODO: Decide if this constructor should be private
    
    CPoint(Vector vPos, Scalar fAngle=0) : m_vPos(vPos), m_fAngle(fAngle)      {}
    void setPos(Vector vVal)                    { m_vPos = vVal; }
    Vector& getPos()                            { return m_vPos; }
    void setAngle(Scalar fVal)                  { m_fAngle = fVal; }
    Scalar& getAngle()                          { return m_fAngle; }
protected:
    Vector m_vPos;
    Scalar m_fAngle;
};

class CSegment
{
    
public:
    
    //CSegment(Vector vPos1, Vector vPos2) : m_pPoint1(0), m_pPoint2(0)
    CSegment() : m_pPoint1(0), m_pPoint2(0)         {}
    CSegment(CPoint* pPoint1, CPoint* pPoint2) : m_pPoint1(0), m_pPoint2(0)
    {
        
        //TODO: Decide if should create new points on the heap or not
        
        init(pPoint1, pPoint2);
        
    }
    
    CPoint* getPoint1()         { return m_pPoint1; }
    CPoint* getPoint2()         { return m_pPoint2; }
    
    bool isSet()                { return m_pPoint1 && m_pPoint2; }
    Scalar getAngle()           { return (m_pPoint2->getPos() - m_pPoint1->getPos()).getAngle(); }
    Scalar getLength()          { return (m_pPoint2->getPos() - m_pPoint1->getPos()).getLength(); }
    Vector getVector()          { return m_pPoint2->getPos() - m_pPoint1->getPos(); }
    Vector getStart()           { return m_pPoint1->getPos(); }
    Vector getEnd()             { return m_pPoint2->getPos(); }
    
private:
    
    void init(CPoint* pPoint1, CPoint* pPoint2)
    {
        m_pPoint1 = pPoint1;
        m_pPoint2 = pPoint2;
    }
    
    CPoint* m_pPoint1;
    CPoint* m_pPoint2;
    
};

//A point with physics so that it can move and rotate
//Constraints constrain these

class CPhysicsPoint : public CPoint
{

private:
    
    //Give CWorld access to the constructor to allow it to make instances
    friend class CWorld;
    
    //TODO: Copy constructor and = operator
    
    CPhysicsPoint(Vector vPos, Scalar fMass = 1.f, Scalar fRotInertia = 1.f) : CPoint(vPos, 0.f)
    {
        init(vPos, fMass, fRotInertia);
    }
    
    void init(Vector vPos, Scalar fMass = 1.f, Scalar fRotInertia = 1.f)
    {
        
        //Initialise physical variables
        m_vOldPos = vPos;
        m_fMass = fMass;
        m_f1overMass = 1.f / m_fMass;
        m_fRotInertia = fRotInertia;
        m_f1overRotInertia = 1.f / m_fRotInertia;
        m_vVel = 0;
        m_vForce = 0;
        m_fAngVel = 0;
        m_fTorque = 0;
        
    }
    
    void cleanup()
    {
        
        
        
    }
    
public:
    
    void setOldPos(Vector vVal)                 { m_vOldPos = vVal; }
    Vector& getOldPos()                         { return m_vOldPos; }
    void setMass(Scalar fVal)                   { m_fMass = fVal; m_f1overMass = 1.f / fVal; }
    Scalar& getMass()                           { return m_fMass; }
    void setRotInertia(Scalar fVal)             { m_fMass = fVal; m_f1overRotInertia = 1.f / fVal; }
    Scalar& getRotInertia()                     { return m_fRotInertia; }
    void setVel(Vector vVal)                    { m_vVel = vVal; }
    Vector& getVel()                            { return m_vVel; }
    void setAngVel(Scalar fVal)                 { m_fAngVel = fVal; }
    Scalar& getAngVel()                         { return m_fAngVel; }
    void setSolid(bool bVal)                    { m_bIsSolid = bVal; }
    bool& isSolid()                             { return m_bIsSolid; }
    //Applies force at vPos in world coordinates
    void applyForce(Vector vForce) { m_vForce += vForce; }
    void applyForce(Vector vForce, Vector vPos) { m_vForce += vForce; m_fTorque += (m_vPos - vPos) ^ vForce; }
    Vector getForce()                           { return m_vForce; }
    
    void update(Scalar fTimeStep);
    
    //TODO: Decide what to do with velocity
    
private:
    
    //Physical variables
    Vector m_vOldPos, m_vVel, m_vForce;
    Scalar m_fAngVel, m_fTorque;
    Scalar m_fMass, m_fRotInertia;
    Scalar m_f1overMass, m_f1overRotInertia;
    bool m_bIsSolid;
    
};

class CPhysicsSegment
{
    
public:
    
    //CSegment(Vector vPos1, Vector vPos2) : m_pPoint1(0), m_pPoint2(0)
    CPhysicsSegment() : m_pPoint1(0), m_pPoint2(0)         {}
    CPhysicsSegment(CPhysicsPoint* pPoint1, CPhysicsPoint* pPoint2) : m_pPoint1(0), m_pPoint2(0)
    {
        
        //TODO: Decide if should create new points on the heap or not
        
        init(pPoint1, pPoint2);
        
    }
    
    CPhysicsPoint* getPoint1()         { return m_pPoint1; }
    CPhysicsPoint* getPoint2()         { return m_pPoint2; }
    
    bool isSet()                { return m_pPoint1 && m_pPoint2; }
    Scalar getAngle()           { return (m_pPoint2->getPos() - m_pPoint1->getPos()).getAngle(); }
    Scalar getLength()          { return (m_pPoint2->getPos() - m_pPoint1->getPos()).getLength(); }
    Vector getVector()          { return m_pPoint2->getPos() - m_pPoint1->getPos(); }
    Vector getStart()           { return m_pPoint1->getPos(); }
    Vector getEnd()             { return m_pPoint2->getPos(); }
    
private:
    
    void init(CPhysicsPoint* pPoint1, CPhysicsPoint* pPoint2)
    {
        m_pPoint1 = pPoint1;
        m_pPoint2 = pPoint2;
    }
    
    CPhysicsPoint* m_pPoint1;
    CPhysicsPoint* m_pPoint2;
    
};

#endif //PHYSICS_H
